#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "wallyingLib.h"


/* G(x) = x7 + x4 + 1, 10010001 */
uint8_t ble_data_whitener(uint8_t poly, uint8_t init, uint8_t data, uint8_t *out)
{
    uint8_t i = 0;
    uint8_t crc = init;
    uint8_t data_in, data_out;

    for (i = 0; i < 8; i++) {
        data_in = bit_get(&data, i);
        data_out = bit_get(&crc, 6) ^ data_in;
        bit_set(out, i, data_out);

        if (crc & 0x40) {
            crc = (crc << 1) ^ poly;
        } else {
            crc <<= 1;
        }
        crc &= 0x7F;
        // printf("Data in : %d, Data out : %d, crc = 0x%02X\r\n", data_in, data_out, crc);
    }

    return crc;
}





uint8_t channel_index = 38;//30;//0;
uint8_t crc_init = 0;

uint8_t input_buf[] =
{
    0x60, 0x1D, 0x4C, 0xBA, 0x95, 0x4D, 0x23, 0xDC, 0x02, 0x01, 0x06, 0x03, 0x02, 0x50, 0xFD, 0x0F, 0x16, 0x50, 0xFD, 0x41, 0x00, 0x00, 0x08, 0x79, 0x71, 0x64, 0x78, 0x6E, 0x65, 0x66, 0x64, 0x08, 0x21, 0x41

    // 0x03, 0x09, 0x08, 0xFF, 0x7F, 0xD9, 0x00, 0x00, 0x00, 0x00, 0x00, 0x39, 0xBA, 0x26,

    // 0xB3, 0x31, 0x11, 0xFB, 0x08, 0x48, 0x08, 0xFB, 0xBC, 0xE7, 0x1B, 0xBC, 0x35, 0xBF, 0xF9, 0xF8
};


int main(int argc, char *argv[])
{
    printf("Hello World!\r\n");


	printf("channel_index = %d (0x%X)\r\n", channel_index, channel_index);



    bit_set(&crc_init, 0, 1);
    bit_set(&crc_init, 1, bit_get(&channel_index, 5));
    bit_set(&crc_init, 2, bit_get(&channel_index, 4));
    bit_set(&crc_init, 3, bit_get(&channel_index, 3));
    bit_set(&crc_init, 4, bit_get(&channel_index, 2));
    bit_set(&crc_init, 5, bit_get(&channel_index, 1));
    bit_set(&crc_init, 6, bit_get(&channel_index, 0));
	printf("crc_init = 0x%X\r\n", crc_init);


    uint8_t crc = 0;
	crc = crc_init;

    for (int i = 0; i < sizeof(input_buf); i++)
    {
        uint8_t whitened = 0;
	    crc = ble_data_whitener(0x11, crc, input_buf[i], &whitened);
        printf("%02X ", whitened);
    }

    printf("\r\n");


    system("pause");
    return 0;
}
